import serial
import array
import struct
import math

ser = serial.Serial()

def startAccessPoint():
    return array.array('B', [0xFF, 0x07, 0x03]).tostring()

def accDataRequest():
    return array.array('B', [0xFF, 0x08, 0x07, 0x00, 0x00, 0x00, 0x00]).tostring()

def initAcc():
    global ser
    ser = serial.Serial(11,115200,timeout=1)
    #Start access point
    ser.write(startAccessPoint())
    ser.write(accDataRequest())
    accel = ser.read(7)


def readAcc():
    global ser
    ser.write(accDataRequest())
    accel = ser.read(7)
    if ord(accel[0]) != 0 and ord(accel[1]) != 0 and ord(accel[2]) != 0:
      gx = ((float) (struct.unpack('b',accel[0])[0]) + 9)/50
      gy = ((float) (struct.unpack('b',accel[1])[0]) + 4)/50
      gz = ((float) (struct.unpack('b',accel[2])[0]) + 20)/50 
      return [gx, gy, gz]
    else:
      return []

def readAngles():
    accel = readAcc()
    if len(accel) == 3:
      gx = accel[0]
      gy = accel[1]
      gz = accel[2]
      rho = math.degrees(math.atan2(gx, math.sqrt(gy*gy + gz*gz)))
      phi = math.degrees(math.atan2(gy, math.sqrt(gx*gx + gz*gz)))
      theta = math.degrees(math.atan2(math.sqrt(gx*gx + gy*gy), gz))
      return [rho, phi, theta]
    else:
      return []